﻿using System;
using System.Collections.Generic;
using System.Text;
using Geometry;
using AGHFiraPlayer;
using AGHFiraPlayer.PreviousDriver;
using AGHFiraPlayer.Drivers;

namespace AGHFiraPlayer.Strategies.StrategiesImpl
{
    class GoalKeeperStrategy : AbstractStrategy
    {
        private RobotController goalKeeper = null;
        
        override public int applicabilityLevel(ref List<IStrategy> choosenStrategies) 
        {
            foreach (AbstractStrategy strategy in choosenStrategies)
            {
                if (strategy.getType() == StrategyType.GoalKeeper)
                {
                    return -1;
                }
            }
            robots.Add(driver.GoalKeeper);
      //      robots.AddRange(driver.Robots);
            goalKeeper = robots[0];
            return 1001; 
        }

        public GoalKeeperStrategy(ref AGHDriver driver)
        {
            robots = new List<RobotController>(1);
            this.driver = driver;
            this.type = StrategyType.GoalKeeper;
        }

        override public void execute()
        {
            double positionError = AGHDriver.ROBOT_DIAMETER;
            double maximumDistanceToKick = AGHDriver.ROBOT_DIAMETER;

            Point2D robotPosition = new Point2D(goalKeeper.robotState.x, goalKeeper.robotState.y);
            Point2D intersectionPoint = Predicator.findBestIntersectionPoint(robotPosition);  
            Point2D ballPos = new Point2D(driver.firaDriver.ballX, driver.firaDriver.ballY);

            double farGoalKeeperPosition = Math.Abs(goalKeeper.Position.x - goalKeeper.startupPosition.x);
            Boolean goalKeeperFarBeyondPenaltyArea = farGoalKeeperPosition > AGHDriver.pitchArea.Width / 3;
            Boolean intersectionPointToFarAway = (double.IsNaN(intersectionPoint.x) || double.IsNaN(intersectionPoint.y)) ?
                                        true : intersectionPoint.GetDistance(goalKeeper.startupPosition) > AGHDriver.pitchArea.Width / 3;

            if (goalKeeperFarBeyondPenaltyArea || intersectionPointToFarAway)
                backToGoalAndRotateToBall();
            else
            {
                if (robotPosition.GetDistance(intersectionPoint) > positionError)
                    GoalKeeperDriver.DriveVelocity(goalKeeper.robotState, intersectionPoint.x, intersectionPoint.y, 15);
                else
                    fixRobotBallAngleAndRotateKick(ballPos, robotPosition, maximumDistanceToKick);
            }
        }

        private void backToGoalAndRotateToBall()
        {
            double yCoord = AGHDriver.home.y;
            Point2D ballPos = new Point2D(driver.firaDriver.ballX, driver.firaDriver.ballY);
            if (Math.Abs(goalKeeper.Position.x - AGHDriver.home.x) < AGHDriver.ROBOT_DIAMETER)
                yCoord = calculateY(ballPos);

            if (backToGoal(yCoord))
                SimpleMovingFunctions.rotateToBall(goalKeeper, driver.firaDriver);
        }

        private Boolean backToGoal(double yCoord)
        {
            Point2D robotPosition = new Point2D(goalKeeper.robotState.x, goalKeeper.robotState.y);
            Point2D target = new Point2D(AGHDriver.home.x, yCoord);
            double distanceToTarget = target.GetDistance(robotPosition);
            if (distanceToTarget > AGHDriver.ROBOT_DIAMETER)
                GoalKeeperDriver.DriveVelocity(goalKeeper.robotState, AGHDriver.home.x, yCoord, 1);
            else
            {
                goalKeeper.Stop();
                return true;
            }
            return false;
        }

        private double calculateY(Point2D ballPos)
        {
            if (ballPos.x > AGHDriver.pitchArea.Width / 2)
                return AGHDriver.pitchArea.Height / 2;

            double calculatedY = ballPos.y;
            if (ballPos.y > AGHDriver.goalEndY)
                calculatedY = AGHDriver.goalEndY;
            else if (ballPos.y < AGHDriver.goalStartY)
                calculatedY = AGHDriver.goalStartY;

            return calculatedY;
        }

        private void fixRobotBallAngleAndRotateKick(Point2D ballPos, Point2D robotPosition, double maximumDistanceToKick)
        {
            double pitchHalfHeight = AGHDriver.pitchArea.Height / 2; ;
            Boolean isInLeftWingSector = ballPos.y < pitchHalfHeight ? true : false;

            SimpleMovingFunctions.rotateToBall(goalKeeper, driver.firaDriver);
            if (robotPosition.GetDistance(ballPos) < maximumDistanceToKick)
                SimpleMovingFunctions.rotateKick(goalKeeper, isInLeftWingSector);
        }
    }
}
